/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "Chain.h"
#include "MotionPlanning.h"

namespace Robotic { namespace Mechanism
{
	using namespace std;
	using namespace Robotic::Math;

	MotionPlanning::MotionPlanning(Chain* pRobot, double stepInterval)
	{
		_pRobot = pRobot;
		_stepInterval = stepInterval;
		_minFlyByZone = 0.010;

		_robotBaseFrame = Frame3::Identity();
		_objectFrame = Frame3::Identity();
		_toolFrame = Frame3::Identity();

		_curRobotJointValues = ColumnVector(_pRobot->Get_Dof());
		_curRobotJointVels = ColumnVector(_pRobot->Get_Dof());
		_curRobotJointAccs = ColumnVector(_pRobot->Get_Dof());
		_curRobotJointTorques = ColumnVector(_pRobot->Get_Dof());

	}

	MotionPlanning::~MotionPlanning()
	{

	}

	double MotionPlanning::GetStepInterval()
	{
		return _stepInterval;
	}

	double MotionPlanning::GetMinFlyByZone()
	{
		return _minFlyByZone;
	}

	Frame3 MotionPlanning::GetRobotBaseFrame()
	{
		return _robotBaseFrame;
	}

	Frame3 MotionPlanning::GetObjectFrame()
	{
		return _objectFrame;
	}

	Frame3 MotionPlanning::GetToolFrame()
	{
		return _toolFrame;
	}

	ColumnVector MotionPlanning::GetCurRobotJointValues()
	{
		return _curRobotJointValues;
	}

	ColumnVector MotionPlanning::GetCurRobotJointVels()
	{
		return _curRobotJointVels;
	}

	ColumnVector MotionPlanning::GetCurRobotJointAccs()
	{
		return _curRobotJointAccs;
	}

	ColumnVector MotionPlanning::GetCurRobotJointTorques()
	{
		return _curRobotJointTorques;
	}

	void MotionPlanning::SetStepInterval(double interval)
	{
		if(interval <= 0)
			throw(new exception(" Argument interval should be > 0."));

		_stepInterval = interval;
	}

	void MotionPlanning::SetMinFlyByZone(double minFlyByZone)
	{
		_minFlyByZone = minFlyByZone;
		if(_minFlyByZone < 0.001)
			_minFlyByZone = 0.001;
	}

	void MotionPlanning::SetRobotBaseFrame(Frame3 baseFrame)
	{
		_robotBaseFrame = baseFrame;
	}
	void MotionPlanning::SetObjectFrame(Frame3 objectFrame)
	{
		_objectFrame = objectFrame;
	}

	void MotionPlanning::SetToolFrame(Frame3 toolFrame)
	{
		_toolFrame = toolFrame;
	}

	void MotionPlanning::SetCurRobotJointValues(ColumnVector values)
	{
		_curRobotJointValues = values;
	}

	void MotionPlanning::SetCurRobotJointVels(ColumnVector values)
	{
		_curRobotJointVels = values;
	}

	void MotionPlanning::SetCurRobotJointAccs(ColumnVector values)
	{
		_curRobotJointAccs = values;
	}

	void MotionPlanning::SetCurRobotJointTorques(ColumnVector values)
	{
		_curRobotJointTorques = values;
	}

}}